package com.fitzpatrickit.robotics.hexaped;

import java.util.Random;

import lejos.robotics.subsumption.Behavior;

public class ExploreBehaviour implements Behavior {
	
	private volatile boolean suppressed = false;
	
	private final HexapedMoveController hexapedMoveController;
	
	private final SonarSampler sonarSampler;
	
	public ExploreBehaviour(HexapedMoveController controller, SonarSampler sonarSampler) {
		this.hexapedMoveController = controller;
		this.sonarSampler = sonarSampler;
	}

	@Override
	public boolean takeControl() {
		return true;
	}

	@Override
	public void action() {
		
		this.suppressed = false;
		
		Random rand = new Random();
		
		while (!this.suppressed) {
			if (this.sonarSampler.getObjectDetected()) {
				
				switch (rand.nextInt(3)) {
				case 0:
					this.hexapedMoveController.turnLeft(20);
					break;
				case 1:
					this.hexapedMoveController.turnRight(20);
					break;
				case 2:
					this.hexapedMoveController.backward(12);
					while (!this.suppressed && this.hexapedMoveController.isMoving()) {
						Thread.yield();
					}
					this.hexapedMoveController.reset();
					switch (rand.nextInt(2)) {
					case 0:
						this.hexapedMoveController.turnLeft(20);
						break;
					case 1:
						this.hexapedMoveController.turnRight(20);
						break;
					default:
						break;
					}
					break;
				default:
					break;
				}
				
				while (!this.suppressed && this.hexapedMoveController.isMoving()) {
					Thread.yield();
				}
				
				this.hexapedMoveController.reset();
			} else {
				this.hexapedMoveController.forward();
				
				while (!this.sonarSampler.getObjectDetected() && !this.suppressed) {
					Thread.yield();
				}
				
				this.hexapedMoveController.stop();
			}
		}
		
		this.hexapedMoveController.stop();
	}

	@Override
	public void suppress() {
		this.suppressed = true;
	}

}
